#include "Compensator.h"
#include <opencv2/core.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp>


namespace hnurm
{

    // 补偿器类
    Compensator::Compensator(const cv::FileNode &cfg_node)
    {
        cfg_node["gravity"] >> gravity_;                    // 从配置文件中读取重力系数
        cfg_node["friction_coeff"] >> friction_coeff_;      // 从配置文件中读取摩擦系数
        cfg_node["bullet_speed_bias"] >> bullet_speed_bias_;// 从配置文件中读取子弹速度偏移
        cfg_node["channel_delay"] >> channel_delay_;        // 从配置文件中读取通道延迟
        cfg_node["imutofrt_y"] >> imutofrt_y;
        cfg_node["imutofrt_z"] >> imutofrt_z;
        fx = fy = fz = 0;                                   // 初始化坐标
        pout = yout = 0;                                    // 初始化输出角度
        tmp_fly_time = channel_delay_;                      // 设置临时飞行时间为通道延迟
        bullet_speed_ = 0;                                  // 初始化子弹速度
    }
    double Compensator::diff_eq(double v) const
    {
        return -gravity_ - (friction_coeff_ * abs(v) * v);
    }
    double Compensator::integrate_euler(double v0, double t)
    {
        double v = v0;
        double y = 0.0;
        double dt = 0.001;
        for (double time = 0; time <= t; time += dt) {
            double dv = diff_eq(v);
            v += dv * dt;
            y += v * dt;
        }
        return y;
    }
    // 判断两个3D点是否在二维平面上
    bool Compensator::cmp(const cv::Point3f &a, const cv::Point3f &b)
    {
        return pow(a.x, 2) + pow(a.y, 2) < pow(b.x, 2) + pow(b.y, 2);
    }

    // 计算最终的角度
    float Compensator::CalcFinalAngle(TargetInfo &target_msg, VisionRecvData &recv_data, VisionSendData &send_data, ArmorsNum &num)
    {
        tmp_fly_time = channel_delay_;
        auto bullet_speed = (float) recv_data.speed;      // 获取接收数据的子弹速度
        auto g_yaw = recv_data.yaw;                       // 获取接收数据的偏航角
        t_info = target_msg;                              // 设置目标消息
        bullet_speed_ = bullet_speed + bullet_speed_bias_;// 计算子弹速度偏移后的速度

        // 迭代
        for (int i = 0; i < 5; i++)
        {
            PredictPose(num); // 根据飞行时间预测目标位置
            CompensatePitch();// 根据目标位置计算补偿角度和新的飞行时间
        }

        yout = (float) (g_yaw + angles::shortest_angular_distance(g_yaw * CV_PI / 180, -atan2(fx_center, fy_center)) * 180 / CV_PI);
        auto yaw_armor = (float) (g_yaw + angles::shortest_angular_distance(g_yaw * CV_PI / 180, -atan2(fx, fy)) * 180 / CV_PI);
        send_data.pitch = pout;
        send_data.yaw = yout;
        return yaw_armor;
    }

    // 根据飞行时间预测目标位置
    void Compensator::PredictPose(ArmorsNum &armors_num)
    {
        if(armors_num == ArmorsNum::ENERGYBUFF_5) {
            if(t_info.if_ready) {
                fx = t_info.x + t_info.w_yaw * t_info.radius_1 * tmp_fly_time*sin(atan2(t_info.z - t_info.z_r, t_info.x - t_info.x_r))/exp(0.9) ;
                fy = t_info.y;
                fz = t_info.z - t_info.w_yaw * t_info.radius_1 * tmp_fly_time*cos(atan2(t_info.z - t_info.z_r, t_info.x - t_info.x_r))/exp(0.9) ;
            }else {
                fx = t_info.x;
                fy = t_info.y;
                fz = t_info.z;
            }
            fx_center = fx;
            fy_center = fy;
            return;
        }
        
        if (abs(t_info.w_yaw) > 1.5 || (abs(t_info.vx) + abs(t_info.vy) + abs(t_info.vz)) > 2) {
            fx = t_info.x + tmp_fly_time * t_info.vx;// 根据飞行时间和速度计算预测的x坐标
            fy = t_info.y + tmp_fly_time * t_info.vy;// 根据飞行时间和速度计算预测的y坐标
            fz = t_info.z + tmp_fly_time * t_info.vz;// 根据飞行时间和速度计算预测的z坐标
            fx_center = fx;
            fy_center = fy;
            float yaw = t_info.yaw + tmp_fly_time * t_info.w_yaw;// 计算预测的偏航角
            cv::Point3f armor1, armor2, armor3, armor4;
            std::vector<cv::Point3f> armor;
            if (armors_num == ArmorsNum::OUTPOST_3) {
                armor1.x = fx - t_info.radius_1 * cos(yaw);                       // 计算预测的护甲点1的x坐标
                armor1.y = fy - t_info.radius_1 * sin(yaw);                       // 计算预测的护甲点1的y坐标
                armor1.z = fz;                                                    // 设置预测的护甲点1的z坐标为当前的z坐标
                armor2.x = fx - t_info.radius_1 * cos(float(yaw + 2 * CV_PI / 3));// 计算预测的护甲点2的x坐标
                armor2.y = fy - t_info.radius_1 * sin(float(yaw + 2 * CV_PI / 3));// 计算预测的护甲点2的y坐标
                armor2.z = fz;                                                    // 设置预测的护甲点2的z坐标为当前的z坐标
                armor3.x = fx - t_info.radius_1 * sin(float(yaw + 4 * CV_PI / 3));// 计算预测的护甲点3的x坐标
                armor3.y = fy - t_info.radius_1 * cos(float(yaw + 4 * CV_PI / 3));// 计算预测的护甲点3的y坐标
                armor3.z = fz;
                armor.push_back(armor1);// 将预测的护甲点1加入vector
                armor.push_back(armor2);// 将预测的护甲点2加入vector
                armor.push_back(armor3);// 将预测的护甲点3加入vector
            } else if (armors_num == ArmorsNum::NORMAL_4) {
                armor1.x = fx - t_info.radius_1 * cos(yaw);// 计算预测的护甲点1的x坐标
                armor1.y = fy - t_info.radius_1 * sin(yaw);// 计算预测的护甲点1的y坐标
                armor1.z = fz;                             // 设置预测的护甲点1的z坐标为当前的z坐标
                armor2.x = fx + t_info.radius_1 * cos(yaw);// 计算预测的护甲点2的x坐标
                armor2.y = fy + t_info.radius_1 * sin(yaw);// 计算预测的护甲点2的y坐标
                armor2.z = fz;                             // 设置预测的护甲点2的z坐标为当前的z坐标
                armor3.x = fx - t_info.radius_2 * sin(yaw);// 计算预测的护甲点3的x坐标
                armor3.y = fy + t_info.radius_2 * cos(yaw);// 计算预测的护甲点3的y坐标
                armor3.z = fz + t_info.dz;                 // 设置预测的护甲点3的z坐标为当前的z坐标加上护甲的深度
                armor4.x = fx + t_info.radius_2 * sin(yaw);// 计算预测的护甲点4的x坐标
                armor4.y = fy - t_info.radius_2 * cos(yaw);// 计算预测的护甲点4的y坐标
                armor4.z = fz + t_info.dz;                 // 设置预测的护甲点4的z坐标为当前的z坐标加上护甲的深度
                armor.push_back(armor1);                   // 将预测的护甲点1加入vector
                armor.push_back(armor2);                   // 将预测的护甲点2加入vector
                armor.push_back(armor3);                   // 将预测的护甲点3加入vector
                armor.push_back(armor4);                   // 将预测的护甲点4加入vector
            } else if (armors_num == ArmorsNum::BALANCE_2) {
                armor1.x = fx - t_info.radius_1 * cos(yaw);// 计算预测的护甲点1的x坐标
                armor1.y = fy - t_info.radius_1 * sin(yaw);// 计算预测的护甲点1的y坐标
                armor1.z = fz;                             // 设置预测的护甲点1的z坐标为当前的z坐标
                armor2.x = fx + t_info.radius_1 * cos(yaw);// 计算预测的护甲点2的x坐标
                armor2.y = fy + t_info.radius_1 * sin(yaw);// 计算预测的护甲点2的y坐标
                armor2.z = fz;                             // 设置预测的护甲点2的z坐标为当前的z坐标
                armor.push_back(armor1);                   // 将预测的护甲点1加入vector
                armor.push_back(armor2);                   // 将预测的护甲点2加入vector
            }
            sort(armor.begin(), armor.end(), cmp);      // 根据护甲点的x,y坐标排序
            double rad0 = atan2(armor[0].y, armor[0].x);// 计算排序后的第一个护甲点的航向角
            double rad1 = atan2(armor[1].y, armor[1].x);
            double rad = atan2(fy_center, fx_center);// 计算预测的航向角
            if (abs(angles::shortest_angular_distance(rad0, rad)) < abs(angles::shortest_angular_distance(rad1, rad)))
            {
                fx = armor[0].x;// 设置预测的x坐标为第一个护甲点的x坐标
                fy = armor[0].y;// 设置预测的y坐标为第一个护甲点的y坐标
                fz = armor[0].z;// 设置预测的z坐标为第一个护甲点的z坐标
            } else
            {
                fx = armor[1].x;// 设置预测的x坐标为第二个护甲点的x坐标
                fy = armor[1].y;// 设置预测的y坐标为第二个护甲点的y坐标
                fz = armor[1].z;// 设置预测的z坐标为第二个护甲点的z坐标
            }
        } else {
            fx = t_info.x - t_info.radius_1 * cos(t_info.yaw);
            fy = t_info.y - t_info.radius_1 * sin(t_info.yaw);
            fz = t_info.z;
            fx_center = fx;
            fy_center = fy;
        }
    }

    // 根据目标位置计算补偿角度和新的飞行时间
    void Compensator::CompensatePitch()
    {
        double dist_horizon = sqrt(pow(fx, 2) + pow(fy - imutofrt_y, 2));// 和目标在水平方向上的距离
        double target_height = fz-imutofrt_z;
        // 迭代参数
        double vx, vy, fly_time, tmp_height = target_height, delta_height = target_height, tmp_pitch, real_height;
#ifdef DEBUG_COMPENSATION
        cout << "init pitch: " << atan2(target_height, dist_horizon) * 180 / M_PI << endl;
#endif// DEBUG_COMPENSATION

        int iteration_num = 0;
        while ((iteration_num <= 50 && abs(delta_height) >= 0.005) || iteration_num <= 5) {
            tmp_pitch = atan((tmp_height) / dist_horizon);
            dist_horizon = sqrt(pow(fx, 2) + pow(fy - imutofrt_y * cos(tmp_pitch)+imutofrt_z* sin(tmp_pitch), 2));
            target_height = fz -imutofrt_z * cos(tmp_pitch)-imutofrt_y* sin(tmp_pitch);
            vx = bullet_speed_ * cos(tmp_pitch);
            vy = bullet_speed_ * sin(tmp_pitch);

            fly_time = (exp(friction_coeff_ * dist_horizon) - 1) / (friction_coeff_ * vx);
            real_height = integrate_euler(vy, fly_time);
            delta_height = target_height - real_height;
            tmp_height += delta_height;
            iteration_num++;

#ifdef DEBUG_COMPENSATION
            cout << "iter: " << i + 1 << " " << delta_height << endl;
#endif// DEBUG_COMPENSATION
        }
#ifdef DEBUG_COMPENSATION
        cout << "res:" << tmp_pitch * 180 / CV_PI << endl;
        cout << "fly_time:" << fly_time << endl;
#endif                                                   // DEBUG_COMPENSATION
        tmp_fly_time = (float) fly_time + channel_delay_;// 计算新的飞行时间
        pout = (float) (tmp_pitch * 180 / CV_PI);        // 计算补偿角度
    }

}// namespace hnurm
